clc
clear
%定义采样次数
ts_sum=100;

%定义采样间隔
ts=0.1;
%%
%位置更新

%定义指示向量
po_sum=[0;0;0];
v_sum=[0;0;0];
euler_sum=[0;0;0];


%定义初始向量
%速度
v_ini=[0;0;0];
v_ini2=[0;0;0];

%位置imu
p_ini=[0;0;0];
p_ini2=[0;0;0];

%姿态
euler_ini=[0;0;0];
att_ini=euler2m(euler_before);

%角速度
w_ini=[0;0;0];
w_ini2=[0;0;0];

%imu输出
up_w=[0;0;0];
up_f=[0;0;0];
for i=1:ts_sum
    [v_1,w_1]=update_vl(v_ini,v_ini2,w_ini,w_ini2,att_ini,p_ini,up_f,up_w,ts);
    p_1=update_p(p_ini,p_ini2,v1,v_ini,ts);
    att_1=update_att(att_ini,v_1,p_1,up_w,ts);

    euler_1=m2euler(att_1);
    euler_sum=[euler_sum,euler_1];
    euler_ini=euler_1;
    
    v_sum=[v_sum,v1];
    v_ini=v1;
    v_ini2=v_ini;
    w_ini=w_1;
    w_ini2=w_ini;

    po_sum=[po_sum,p_1];
    p_ini=p_1;
    p_ini2=p_ini;
end
